/*
 * Part of the program control-test
 * Keeps a constant distance in front of the robot
 */
#include <xtask.h>
#include <rrcon.h>
#include <log.h>
#include <printfstring.h>

#include "sensors_read.h"
#include "control_distance.h"

#define PERIOD 10       // Hz
#define WAIT_QUEUE 0.1  // Max number of sec of an item in the queue to pick

#define LMOTOR 0
#define RMOTOR 3

#define SPEED 500

namespace control_test {
    using namespace kthrobot;

    DistanceControler::DistanceControler(SensorsDataQueue *data_queue)
        : XTask("DistanceControler"),data_queue_(data_queue), target_set_(false){
            SetPeriodHz(PERIOD);
        }

    DistanceControler::~DistanceControler() {
        Join();
    }

    void DistanceControler::TaskCode() {
        while(!MustStop()) {
            if (!target_set_) { continue; }  // If no target do nothing 
            
            // Very basic feedback loop:
            // - go forward if more than 10 cm
            // - go backward if less than 5 cm
            int distance = get_current_distance();
            Log::Write("Measured distance: %d cm", distance);
            if (distance > 30) {
                set_both_motors(SPEED);
            } else if (distance < 20 && distance > -1) {
                set_both_motors(-SPEED);
            }

            WaitPeriod();
        }
    }

    void DistanceControler::set_target(int sensor, int distance_cm) {
        // Set the target distance the robot should have from the obstacle
        target_set_ = true;
        target_sensor_ = sensor;
        target_distance_ = distance_cm;
    }

    int DistanceControler::get_current_distance() {
        // Compute the current distance from the sensors data
        ADCRawData data;
        if (data_queue_->Deq(&data, 0.1)) {
            return SensorsReader::ConvertToCm(data.channel[target_sensor_],0);
        } else {
            return -1;
        }
    }

    void DistanceControler::set_both_motors(int speed) {
        Log::Write("Setting speed to %i", speed);
        RRCon::motor(LMOTOR).SetSpeed(-speed);
        RRCon::motor(RMOTOR).SetSpeed(speed);
    }

};
